import os
from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 获取apriltag_ros包的路径
    apriltag_ros_share_dir = get_package_share_directory('apriltag_ros')
    
    

    # 创建ComposableNode列表
    composable_nodes = [
        ComposableNode(
            package='v4l2_camera',
            plugin='v4l2_camera::V4L2Camera',
            name='camera',
            namespace='v4l2',
            parameters=[{'video_device': '/dev/video0'}],
            remappings=[
                ('/v4l2/image', '/v4l2/image_raw'),
                ('/v4l2/camera_info', '/v4l2/camera_info')
            ]
        ),
        ComposableNode(
            package='image_proc',
            plugin='image_proc::RectifyNode',
            name='rectify',
            namespace='v4l2',
            remappings=[
                ('/v4l2/image', '/v4l2/image_raw'),
                ('/v4l2/camera_info', '/v4l2/camera_info')
            ]
        ),
        ComposableNode(
            package='apriltag_ros',
            plugin='AprilTagNode',
            name='apriltag',
            namespace='apriltag',
            parameters=[os.path.join(apriltag_ros_share_dir, 'cfg', 'tags_36h11.yaml')],
            remappings=[
                ('/apriltag/image_rect', '/v4l2/image_raw'),
                ('/apriltag/camera_info', '/v4l2/camera_info')
            ]
        )
    ]

    # 创建ComposableNodeContainer
    container = ComposableNodeContainer(
        name='apriltag_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=composable_nodes
    )

    return LaunchDescription([container])
